import numpy
import rosbag
import cv2
import os
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

rgb_left_path = '/media/atakanoinu/MSDAS/[3]dorm3/[6]dorm3_dusk_cloudy/rgb/camera_left/'
rgb_right_path = '/media/atakanoinu/MSDAS/[3]dorm3/[6]dorm3_dusk_cloudy/rgb/camera_right/'
event_left_path = '/media/atakanoinu/MSDAS/[3]dorm3/[6]dorm3_dusk_cloudy/event/event_left/'
event_right_path = '/media/atakanoinu/MSDAS/[3]dorm3/[6]dorm3_dusk_cloudy/event/event_right/'
rgbd_path = '/media/atakanoinu/MSDAS/[3]dorm3/[6]dorm3_dusk_cloudy/rgbd/'
ir_path = '/media/atakanoinu/MSDAS/[3]dorm3/[6]dorm3_dusk_cloudy/ir/'

bridge = CvBridge()
with rosbag.Bag('/media/atakanoinu/MSDAS/[3]dorm3/[6]dorm3_dusk_cloudy/dorm3_dusk_cloudy.bag', 'r') as bag:
    num_rgb_left = 1
    num_rgb_right = 1
    num_event_left = 1
    num_event_right = 1
    num_rgbd = 1
    num_ir = 1
    for topic,msg,t in bag.read_messages():
        if topic == "/camera/image_left":
            cv_image = bridge.imgmsg_to_cv2(msg, 'bgr8')
            num_rgb_left = "%.6f" %  msg.header.stamp.to_sec()
            image_name = str(num_rgb_left) + '.png'
            cv2.imwrite(rgb_left_path + image_name, cv_image)  
            print(rgb_left_path + image_name)

        if topic == "/camera/image_right":
            cv_image = bridge.imgmsg_to_cv2(msg, 'bgr8')
            num_rgb_right = "%.6f" %  msg.header.stamp.to_sec()
            image_name = str(num_rgb_right) + '.png'
            cv2.imwrite(rgb_right_path + image_name, cv_image)  
            print(rgb_right_path + image_name)

        if topic == "/event_camera/img_left":
            cv_image = bridge.imgmsg_to_cv2(msg, 'bgr8')
            # num_event_left = "%.6f" %  msg.header.stamp.to_sec()
            image_name = str(num_event_left) + '.png'
            cv2.imwrite(event_left_path + image_name, cv_image)  
            print(event_left_path + image_name)
            num_event_left += 1

        if topic == "/event_camera/img_right":
            cv_image = bridge.imgmsg_to_cv2(msg, 'bgr8')
            # num_event_right = "%.6f" %  msg.header.stamp.to_sec()
            image_name = str(num_event_right) + '.png'
            cv2.imwrite(event_right_path + image_name, cv_image)  
            print(event_right_path + image_name)
            num_event_right += 1

        if topic == "/rgbd/color/image":
            cv_image = bridge.imgmsg_to_cv2(msg, 'bgr8')
            num_rgbd = "%.6f" %  msg.header.stamp.to_sec()
            image_name = str(num_rgbd) + '.png'
            cv2.imwrite(rgbd_path + image_name, cv_image)  
            print(rgbd_path + image_name)

        if topic == "/rgbd/ir/image":
            cv_image = bridge.imgmsg_to_cv2(msg, '16UC1')
            num_ir = "%.6f" %  msg.header.stamp.to_sec()
            image_name = str(num_ir) + '.png'
            cv2.imwrite(ir_path + image_name, cv_image)  
            print(ir_path + image_name)
